In [1]:
import numpy as np
import cv2
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
import os
import glob
from pprint import pprint
Helpers¶
In [2]:
def draw_lines(img, lines, color=[255, 0, 0], thickness=2):
for line in lines:
x1,y1,x2,y2 = line
cv2.line(img, (x1, y1), (x2, y2), color, thickness)
def draw_vertices(img, vertices, color=[255, 0, 0], thickness=2):
v = np.copy(vertices)
v = np.concatenate((v, [v[0]]), axis=0)
lines = [(v[i][0], v[i][1], v[i+1][0], v[i+1][1]) for i in range(len(v)-1)]
draw_lines(img, lines, color, thickness)
Compute the camera calibration matrix and distortion coefficients given a set of chessboard images.¶
In [3]:
# prepare object points
nx = 9
ny = 6
def getCalibrationPoints(images):
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((nx*ny,3), np.float32)
objp[:,:2] = np.mgrid[0:nx,0:ny].T.reshape(-1,2)
for fname in images:
img = cv2.imread(fname)
# Convert to grayscale
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find the chessboard corners
ret, corners = cv2.findChessboardCorners(gray, (nx, ny), None)
# If save imgpoints
if ret == True:
imgpoints.append(corners)
objpoints.append(objp)
img_shape = img.shape
return imgpoints, objpoints, img_shape
def getCalibrationMatrixAndDistortionCoefficients(images):
imgpoints, objpoints, img_shape = getCalibrationPoints(images)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
objpoints,
imgpoints,
img_shape[1::-1],
None,
None
)
return mtx, dist
# Test
cal_images = glob.glob('camera_cal/*.jpg')
mtx, dist = getCalibrationMatrixAndDistortionCoefficients(cal_images)
# draw corners
img = cv2.imread(cal_images[13])
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (nx, ny), None)
cv2.drawChessboardCorners(img, (nx, ny), corners, ret)
# undistort
dst = cv2.undistort(img, mtx, dist, None, mtx)
bgr = cv2.cvtColor(dst, cv2.COLOR_RGB2BGR)
cv2.imwrite('output_images/calibration_dist_correction.jpg', bgr)
# plot
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(img)
ax1.set_title('Original Image', fontsize=50)
ax2.imshow(dst)
ax2.set_title('Undistorted Image', fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
Apply a distortion correction to raw images.¶
Save corrected images in output_images prefixed with dist_correction_
In [4]:
raw_images = os.listdir("test_images/")
no_of_images = len(raw_images)
for i in range(no_of_images):
img = mpimg.imread("test_images/" + raw_images[i])
dst = cv2.undistort(img, mtx, dist, None, mtx)
bgr = cv2.cvtColor(dst, cv2.COLOR_RGB2BGR)
cv2.imwrite('output_images/dist_correction_' + raw_images[i], bgr)
# plot
fig = plt.figure(figsize=(24, 80))
fig.tight_layout()
fig.add_subplot(no_of_images,2,1 + i*2)
plt.imshow(img)
fig.add_subplot(no_of_images,2,2 + i*2)
plt.imshow(dst)
Use color transforms, gradients, etc., to create a thresholded binary image.¶
In [5]:
# Define a function that applies Sobel x or y,
# then takes an absolute value and applies a threshold.
# Note: calling your function with orient='x', thresh_min=5, thresh_max=100
# should produce output like the example image shown above this quiz.
def abs_sobel_thresh(gray, orient='x', sobel_kernel=3, thresh=(0, 255)):
# Apply the following steps to img
# 2) Take the derivative in x or y given orient = 'x' or 'y'
if orient == 'x':
sobel = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
if orient == 'y':
sobel = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
# 3) Take the absolute value of the derivative or gradient
abs_sobel = np.absolute(sobel)
# 4) Scale to 8-bit (0 - 255) then convert to type = np.uint8
scaled_sobel = np.uint8(255*abs_sobel/np.max(abs_sobel))
# 5) Create a mask of 1's where the scaled gradient magnitude
# is > thresh_min and < thresh_max
binary_output = np.zeros_like(scaled_sobel)
binary_output[(scaled_sobel >= thresh[0]) & (scaled_sobel <= thresh[1])] = 1
# 6) Return this mask as your binary_output image
return binary_output
# Define a function that applies Sobel x and y,
# then computes the magnitude of the gradient
# and applies a threshold
def mag_threshold(gray, sobel_kernel=3, mag_thresh=(0, 255)):
# Apply the following steps to img
# 2) Take the gradient in x and y separately
sobel_x = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
sobel_y = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
# 3) Calculate the magnitude
abs_sobel= np.sqrt(sobel_x**2 + sobel_y**2)
# 4) Scale to 8-bit (0 - 255) and convert to type = np.uint8
scaled_sobel = np.uint8(255*abs_sobel/np.max(abs_sobel))
# 5) Create a binary mask where mag thresholds are met
binary_output = np.zeros_like(scaled_sobel)
binary_output[(scaled_sobel >= mag_thresh[0]) & (scaled_sobel <= mag_thresh[1])] = 1
# 6) Return this mask as your binary_output image
return binary_output
# Define a function that applies Sobel x and y,
# then computes the direction of the gradient
# and applies a threshold.
def dir_threshold(gray, sobel_kernel=3, dir_thresh=(0, np.pi/2)):
# Apply the following steps to img
# 2) Take the gradient in x and y separately
sobel_x = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
sobel_y = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
# 3) Take the absolute value of the x and y gradients
abs_sobelx = np.abs(sobel_x)
abs_sobely = np.abs(sobel_y)
# 4) Use np.arctan2(abs_sobely, abs_sobelx) to calculate the direction of the gradient
direction = np.arctan2(abs_sobely, abs_sobelx)
# 5) Create a binary mask where direction thresholds are met
binary_output = np.zeros_like(direction)
binary_output[(direction >= dir_thresh[0]) & (direction <= dir_thresh[1])] = 1
# 6) Return this mask as your binary_output image
return binary_output
def thresholds(gray, kernel=3, thresh=(0, 255)):
blur = cv2.blur(gray, (kernel, kernel))
binary = np.zeros_like(blur)
binary[(blur >= thresh[0]) & (blur <= thresh[1])] = 1
return binary
def region_of_interest(img, vertices):
"""
Applies an image mask.
Only keeps the region of the image defined by the polygon
formed from `vertices`. The rest of the image is set to black.
`vertices` should be a numpy array of integer points.
"""
#defining a blank mask to start with
mask = np.zeros_like(img)
#defining a 3 channel or 1 channel color to fill the mask with depending on the input image
if len(img.shape) > 2:
channel_count = img.shape[2] # i.e. 3 or 4 depending on your image
ignore_mask_color = (255,) * channel_count
else:
ignore_mask_color = 255
#filling pixels inside the polygon defined by "vertices" with the fill color
cv2.fillPoly(mask, vertices, ignore_mask_color)
#returning the image only where mask pixels are nonzero
masked_image = cv2.bitwise_and(img, mask)
return masked_image
def combining_thresholds(img, kernel=3,
x_thresh=(0, 255),
y_thresh=(0, 255),
mag_thresh=(0, 255),
dir_thresh=(0, np.pi/2),
s_thresh=(0, 255)
):
# Convert to HLS color space and separate the V channel
hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)
l_channel = hls[:,:,1]
s_channel = hls[:,:,2]
gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
# Apply each of the thresholding functions
gradx = abs_sobel_thresh(l_channel, orient='x', sobel_kernel=kernel, thresh=x_thresh)
grady = abs_sobel_thresh(l_channel, orient='y', sobel_kernel=kernel, thresh=y_thresh)
#mag_binary = mag_threshold(l_channel, sobel_kernel=kernel, mag_thresh=mag_thresh)
#dir_binary = dir_threshold(l_channel, sobel_kernel=kernel, dir_thresh=dir_thresh)
s_binary = thresholds(s_channel, kernel=kernel, thresh=s_thresh)
combined = np.zeros_like(gradx)
combined[
((gradx == 1) & (grady == 1)) |
#((mag_binary == 1) & (dir_binary == 1)) |
s_binary == 1
] = 1
#combined = np.dstack((s_binary, (mag_binary == 1) & (dir_binary == 1), (gradx == 1) & (grady == 1))) * 255
return combined
In [6]:
cor_images = glob.glob('output_images/dist_correction_*.jpg')
cor_images.sort()
no_of_images = len(cor_images)
def getVerticesRoi(img):
# roi
top_roi = 450
top_width_roi = 160
left_roi = 170
right_roi = 140
bottom_roi = 20
image_size = img.shape[1::-1]
return np.array([[
(left_roi, image_size[1]-bottom_roi),
(image_size[0]/2-top_width_roi/2, top_roi),
(image_size[0]/2+top_width_roi/2, top_roi),
(image_size[0]-right_roi, image_size[1]-bottom_roi)
]], dtype=np.int32)
img = mpimg.imread(cor_images[0])
vertices_roi = getVerticesRoi(img)
for i in range(no_of_images):
#for i in [0]:
img = mpimg.imread(cor_images[i])
dst = combining_thresholds(
img,
kernel=5,
x_thresh=(30, 255),
y_thresh=(30, 255),
mag_thresh=(50, 255),
dir_thresh=(0.1, 1.4),
s_thresh=(140, 255)
)
dst = region_of_interest(dst, vertices_roi)
color_binary = np.dstack((dst, dst, dst)) * 255
fname = cor_images[i].replace('dist_correction_', 'binary_');
cv2.imwrite(fname, color_binary)
# plot
fig = plt.figure(figsize=(24, 80))
fig.tight_layout()
fig.add_subplot(no_of_images,2,1)
x = vertices_roi[:,:,0].flatten()
y = vertices_roi[:,:,1].flatten()
plt.plot(x, y, 'b--', lw=1)
plt.imshow(img)
fig.add_subplot(no_of_images,2,2)
plt.imshow(dst, cmap='gray')
#plt.imshow(dst)
Apply a perspective transform to rectify binary image¶
In [7]:
src_vertices = np.float32([
[590, 452],
[690, 452],
[1120, 720],
[194, 720]
])
dst_vertices = np.float32([
[294, 0],
[1020, 0],
[1020, 720],
[294, 720]
])
M = cv2.getPerspectiveTransform(src_vertices, dst_vertices)
Minv = cv2.getPerspectiveTransform(dst_vertices, src_vertices)
img = mpimg.imread(cor_images[0])
img_size = img.shape[1::-1]
warped = cv2.warpPerspective(img, M, img_size, flags=cv2.INTER_LINEAR)
draw_vertices(img, src_vertices, thickness=2)
draw_vertices(warped, dst_vertices, thickness=2)
# plot
fig = plt.figure(figsize=(80, 80))
fig.add_subplot(111)
plt.imshow(img)
bgr = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
cv2.imwrite('output_images/perspective_image.jpg', bgr)
bgr = cv2.cvtColor(warped, cv2.COLOR_RGB2BGR)
cv2.imwrite('output_images/perspective_warped.jpg', bgr)
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(img)
ax1.set_title('Original Image', fontsize=50)
ax2.imshow(warped)
ax2.set_title('Transform Image', fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
bin_images = glob.glob('output_images/binary_*.jpg')
bin_images.sort()
no_of_images = len(bin_images)
for i in range(no_of_images):
#for i in [0]:
img = mpimg.imread(bin_images[i])
warped = cv2.warpPerspective(img, M, img_size, flags=cv2.INTER_LINEAR)
fname = bin_images[i].replace('binary_', 'warped_');
cv2.imwrite(fname, warped)
draw_vertices(img, src_vertices, thickness=2)
draw_vertices(warped, dst_vertices, thickness=2)
# plot
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(img)
ax2.imshow(warped)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
Detect lane pixels and fit to find the lane boundary¶
Finding the Lines: Sliding Window¶
In [8]:
# Presumption
# - the projection of the lane is about 30 meters long
# - the lane is about 3.7 meters wide
# Define conversions in x and y from pixels space to meters
xm_per_pix = 3.7/726 # meters per pixel in x dimension (1020 - 294 = 726)
ym_per_pix = 30.0/720 # meters per pixel in y dimension
class Line():
def __init__(self, shape, xm_per_pix, ym_per_pix):
# max last n lines saved
self.max_n_lines = 5
# was the line detected in the last iteration?
self.detected = False
# x values of the last n fits of the line
self.recent_xfitted = []
#average x values of the fitted line over the last n iterations
self.bestx = None
# the last n fits of the line
self.recent_fitted = []
#polynomial coefficients averaged over the last n iterations
self.best_fit = None
#polynomial coefficients for the most recent fit
self.current_fit = [np.array([False])]
#radius of curvature of the line in some units
self.radius_of_curvature = None
#distance in meters of vehicle center from the line
self.line_base_pos = None
#difference in fit coefficients between last and new fits
self.diffs = np.array([0,0,0], dtype='float')
#x values for detected line pixels
self.allx = None
#y values for detected line pixels
self.ally = None
# the shape of the line image
self.shape = shape
# meters per pixel in y dimension
self.ym_per_pix = ym_per_pix
# meters per pixel in x dimension
self.xm_per_pix = xm_per_pix
# bottom of your image
self.bottom_y = self.shape[0] - 1
def props(self):
return vars(self)
def init(self, allx, ally):
self.fitPolynomial(allx, ally)
self.updateLineBasePos()
self.updateRadiusOfCurvature()
# calculate current_fit
def fitPolynomial(self, allx, ally):
line_detected = len(allx) != 0
# Fit new polynomials
fit = None
bestx = None
recent_xfitted = []
recent_fitted = []
if line_detected == True:
fit = np.polyfit(ally, allx, 2)
y_eval = self.bottom_y
bestx = fit[0]*y_eval**2 + fit[1]*y_eval + fit[2]
recent_xfitted = [bestx]
recent_fitted = [fit]
self.detected = line_detected
self.allx = allx
self.ally = ally
self.current_fit = fit
self.best_fit = fit
self.bestx = bestx
self.recent_xfitted = recent_xfitted
self.recent_fitted = recent_fitted
def append(self, line):
if line.detected is False:
raise Exception('do not append an undetected line')
newLine = Line(self.shape, self.xm_per_pix, self.ym_per_pix)
newLine.__dict__.update(self.__dict__)
newLine.recent_xfitted = list(newLine.recent_xfitted)
newLine.recent_xfitted.append(line.bestx)
if len(newLine.recent_xfitted) > self.max_n_lines:
newLine.recent_xfitted = newLine.recent_xfitted[1:]
newLine.bestx = np.average(newLine.recent_xfitted)
newLine.recent_fitted = list(newLine.recent_fitted)
newLine.recent_fitted.append(line.best_fit)
if len(newLine.recent_fitted) > self.max_n_lines:
newLine.recent_fitted = newLine.recent_fitted[1:]
newLine.current_fit = line.current_fit
newLine.best_fit = np.average(newLine.recent_fitted, axis=0)
newLine.diffs = newLine.best_fit - line.best_fit
newLine.allx = line.allx
newLine.ally = line.ally
newLine.updateLineBasePos()
newLine.updateRadiusOfCurvature()
return newLine
# use best_fit to estimate line_base_pos
def updateLineBasePos(self):
if self.best_fit is None:
return
# calculate line_base_pos
y_eval = self.bottom_y
fitx = self.fitX(y_eval)
self.line_base_pos = abs(fitx - self.shape[1]/2)*self.xm_per_pix
# use best_fit to estimate radius_of_curvature
def updateRadiusOfCurvature(self):
if self.best_fit is None:
return
# scale polynomial coefficients A and B to fit in meters
a_scale = self.xm_per_pix/(self.ym_per_pix**2)
b_scale = self.xm_per_pix/self.ym_per_pix
# calculate radius_of_curvature
y_eval = self.bottom_y
numerator = ((1 + (2*a_scale*self.best_fit[0]*y_eval*self.ym_per_pix + b_scale*self.best_fit[1])**2)**1.5)
denominator = np.absolute(2*a_scale*self.best_fit[0])
self.radius_of_curvature = numerator/denominator
# Generate x and y values for plotting
def generateXY(self, shape):
ploty = np.linspace(0, shape[0]-1, shape[0])
return self.fitX(ploty), ploty
# use best_fit to fit x from y
def fitX(self, ploty):
try:
fitx = self.best_fit[0]*ploty**2 + self.best_fit[1]*ploty + self.best_fit[2]
except TypeError:
# Avoids an error if `left` and `right_fit` are still none or incorrect
# print('The function failed to fit a line!')
fitx = 1*ploty**2 + 1*ploty
return fitx
def find_lane_pixels(binary_warped):
# Take a histogram of the bottom half of the image
histogram = np.sum(binary_warped[binary_warped.shape[0]//2:,:], axis=0)
# Find the peak of the left and right halves of the histogram
# These will be the starting point for the left and right lines
midpoint = np.int(histogram.shape[0]//2)
leftx_base = np.argmax(histogram[:midpoint])
rightx_base = np.argmax(histogram[midpoint:]) + midpoint
# HYPERPARAMETERS
# Choose the number of sliding windows
nwindows = 9
# Set the width of the windows +/- margin
margin = 100
# Set minimum number of pixels found to recenter window
minpix = 60
# Set height of windows - based on nwindows above and image shape
window_height = np.int(binary_warped.shape[0]//nwindows)
# Identify the x and y positions of all nonzero pixels in the image
nonzero = binary_warped.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
# Current positions to be updated later for each window in nwindows
leftx_current = leftx_base
rightx_current = rightx_base
# Create empty lists to receive left and right lane pixel indices
left_lane_inds = []
right_lane_inds = []
# Step through the windows one by one
for window in range(nwindows):
# Identify window boundaries in x and y (and right and left)
win_y_low = binary_warped.shape[0] - (window+1)*window_height
win_y_high = binary_warped.shape[0] - window*window_height
### TO-DO: Find the four below boundaries of the window ###
win_xleft_low = leftx_current - margin # Update this
win_xleft_high = leftx_current + margin # Update this
win_xright_low = rightx_current - margin # Update this
win_xright_high = rightx_current + margin # Update this
### TO-DO: Identify the nonzero pixels in x and y within the window ###
good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) &
(nonzerox >= win_xleft_low) & (nonzerox < win_xleft_high)).nonzero()[0]
good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) &
(nonzerox >= win_xright_low) & (nonzerox < win_xright_high)).nonzero()[0]
# Append these indices to the lists
left_lane_inds.append(good_left_inds)
right_lane_inds.append(good_right_inds)
if len(good_left_inds) > minpix:
leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
if len(good_right_inds) > minpix:
rightx_current = np.int(np.mean(nonzerox[good_right_inds]))
# Concatenate the arrays of indices (previously was a list of lists of pixels)
try:
left_lane_inds = np.concatenate(left_lane_inds)
right_lane_inds = np.concatenate(right_lane_inds)
except ValueError:
# Avoids an error if the above is not implemented fully
pass
# Extract left and right line pixel positions
leftx = nonzerox[left_lane_inds]
lefty = nonzeroy[left_lane_inds]
rightx = nonzerox[right_lane_inds]
righty = nonzeroy[right_lane_inds]
return leftx, lefty, rightx, righty
def fitSlidingWindow(binary_warped):
# Find our lane pixels first
leftx, lefty, rightx, righty = find_lane_pixels(binary_warped)
shape = binary_warped.shape
leftLane = Line(shape, xm_per_pix, ym_per_pix)
leftLane.init(leftx, lefty)
rightLane = Line(shape, xm_per_pix, ym_per_pix)
rightLane.init(rightx, righty)
return leftLane, rightLane
def drawLane(image, warped, Minv, leftLane, rightLane):
left_fitx, left_ploty = leftLane.generateXY(warped.shape)
right_fitx, right_ploty = rightLane.generateXY(warped.shape)
# Create an image to draw the lines on
warp_zero = np.zeros_like(warped).astype(np.uint8)
color_warp = np.dstack((warp_zero, warp_zero, warp_zero))
# Recast the x and y points into usable format for cv2.fillPoly()
pts_left = np.array([np.transpose(np.vstack([left_fitx, left_ploty]))])
pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, right_ploty])))])
pts = np.hstack((pts_left, pts_right))
# Draw the lane onto the warped blank image
cv2.fillPoly(color_warp, np.int_([pts]), (0,255, 0))
newwarp = np.dstack((warped, warped, warped))
lane_warped = cv2.addWeighted(newwarp, 1, color_warp, 0.3, 0)
# Warp the blank back to original image space using inverse perspective matrix (Minv)
newwarp = cv2.warpPerspective(color_warp, Minv, (image.shape[1], image.shape[0]))
# Combine the result with the original image
lane_image = cv2.addWeighted(image, 1, newwarp, 0.3, 0)
return lane_image, lane_warped
Warp the detected lane boundaries back onto the original image.¶
In [9]:
warped_images = glob.glob('output_images/warped_*.jpg')
warped_images.sort()
no_of_images = len(warped_images)
sliding_lanes = []
for i in range(no_of_images):
#for i in [0]:
img = mpimg.imread(warped_images[i])
gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
leftLane, rightLane = fitSlidingWindow(gray)
sliding_lanes.append((leftLane, rightLane))
cor_image = mpimg.imread(cor_images[i])
lane_image, lane_warped = drawLane(cor_image, gray, Minv, leftLane, rightLane)
# plot
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(lane_warped)
ax2.imshow(lane_image)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
Finding the Lines: Search from Prior¶
In [10]:
def search_around_poly(binary_warped, left_fit, right_fit):
# HYPERPARAMETER
# Choose the width of the margin around the previous polynomial to search
margin = 50
# Grab activated pixels
nonzero = binary_warped.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
left_lane_inds = ((nonzerox > (left_fit[0]*(nonzeroy**2) + left_fit[1]*nonzeroy +
left_fit[2] - margin)) & (nonzerox < (left_fit[0]*(nonzeroy**2) +
left_fit[1]*nonzeroy + left_fit[2] + margin)))
right_lane_inds = ((nonzerox > (right_fit[0]*(nonzeroy**2) + right_fit[1]*nonzeroy +
right_fit[2] - margin)) & (nonzerox < (right_fit[0]*(nonzeroy**2) +
right_fit[1]*nonzeroy + right_fit[2] + margin)))
# Again, extract left and right line pixel positions
leftx = nonzerox[left_lane_inds]
lefty = nonzeroy[left_lane_inds]
rightx = nonzerox[right_lane_inds]
righty = nonzeroy[right_lane_inds]
return leftx, lefty, rightx, righty
def fitAroundPoly(binary_warped, leftLane, rightLane):
leftx, lefty, rightx, righty = search_around_poly(binary_warped, leftLane.best_fit, rightLane.best_fit)
shape = binary_warped.shape
leftLane = Line(shape, xm_per_pix, ym_per_pix)
leftLane.init(leftx, lefty)
rightLane = Line(shape, xm_per_pix, ym_per_pix)
rightLane.init(rightx, righty)
return leftLane, rightLane
Warp the detected lane boundaries back onto the original image.¶
In [11]:
prior_lanes = []
for i in range(no_of_images):
#for i in [0]:
img = mpimg.imread(warped_images[i])
gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
leftOldLane, rightOldLane = sliding_lanes[i]
leftLane, rightLane = fitAroundPoly(gray, leftOldLane, rightOldLane)
prior_lanes.append((leftLane, rightLane))
cor_image = mpimg.imread(cor_images[i])
lane_image, lane_warped = drawLane(cor_image, gray, Minv, leftLane, rightLane)
fname = warped_images[i].replace('warped_', 'lane_warped_');
cv2.imwrite(fname, lane_warped)
fname = warped_images[i].replace('warped_', 'lane_image_');
bgr_lane_image = cv2.cvtColor(lane_image, cv2.COLOR_RGB2BGR)
cv2.imwrite(fname, bgr_lane_image)
# plot
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(lane_warped)
ax2.imshow(lane_image)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
Determine the curvature of the lane and vehicle position with respect to center¶
Curvature of the lane and vehicle position are both implemented as methods on the Line class. xm_per_pix and ym_per_pix Define conversions in x and y from pixels space to meters. The constants a_scale and b_scale is needed to convert x and y from pixels space to meters.
Presumption¶
- the projection of the lane is about 30 meters long
- the lane is about 3.7 meters wide
# use best_fit to estimate line_base_pos
def updateLineBasePos(self):
if self.best_fit is None:
return
# calculate line_base_pos
y_eval = self.bottom_y
fitx = self.fitX(y_eval)
self.line_base_pos = abs(fitx - self.shape[1]/2)*self.xm_per_pix
# use best_fit to estimate radius_of_curvature
def updateRadiusOfCurvature(self):
if self.best_fit is None:
return
# scale polynomial coefficients A and B to fit in meters
a_scale = self.xm_per_pix/(self.ym_per_pix**2)
b_scale = self.xm_per_pix/self.ym_per_pix
# calculate radius_of_curvature
y_eval = self.bottom_y
numerator = ((1 + (2*a_scale*self.best_fit[0]*y_eval*self.ym_per_pix + b_scale*self.best_fit[1])**2)**1.5)
denominator = np.absolute(2*a_scale*self.best_fit[0])
self.radius_of_curvature = numerator/denominator
Output visual display of the lane boundaries and numerical estimation of lane curvature and vehicle position.¶
In [12]:
lane_images = glob.glob('output_images/lane_image_*.jpg')
lane_images.sort()
no_of_images = len(lane_images)
def putText(img, text, pos, font=cv2.FONT_HERSHEY_SIMPLEX, fontScale=2, fontColor=(255,255,255), lineType=2):
cv2.putText(img, text, pos, font, fontScale, fontColor, lineType)
# annotate an image with radius_of_curvature and line_base_pos for a lane
def annotate(img, left_line, right_line):
if not left_line.detected or not right_line.detected:
return
radius_of_curvature = (left_line.radius_of_curvature + right_line.radius_of_curvature)/2
putText(img, 'Radius of Curvature {0:.0f}(m)'.format(radius_of_curvature), (50, 60))
center = (left_line.line_base_pos + right_line.line_base_pos)/2
line_base_pos = center - left_line.line_base_pos
leftRight = 'left' if line_base_pos > 0 else 'right'
putText(img, 'Vehicle is {0:.2f}m {1} of center'.format(abs(line_base_pos), leftRight), (50, 120))
for i in range(no_of_images):
#for i in [0]:
img = mpimg.imread(lane_images[i])
annotate(img, prior_lanes[i][0], prior_lanes[i][1])
fname = lane_images[i].replace('lane_image_', 'annotate_');
bgr_annotate_image = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
cv2.imwrite(fname, bgr_annotate_image)
# plot
fig = plt.figure(figsize=(20, 20))
fig.add_subplot(111)
plt.imshow(img)
In [13]:
# Import everything needed to edit/save/watch video clips
from moviepy.editor import VideoFileClip
from IPython.display import HTML
In [14]:
current_left_line = None
current_right_line = None
failed_attempts = 0
stats = {
'fitSlidingWindow': 0,
'fitAroundPoly': 0,
'error': 0,
'reset': 0,
'missingLeftLine': 0,
'missingRightLine': 0
}
def find_lane(img, max_failed_attempts):
global current_left_line
global current_right_line
global failed_attempts
global stats
# the find lane pipeline consist of following steps
# 1. resize to (1280, 720)
scaled_img = cv2.resize(img, (1280, 720))
# 2. undistort image
undistort_img = cv2.undistort(scaled_img, mtx, dist, None, mtx)
# 3. make binary image
binary_img = combining_thresholds(
undistort_img,
kernel=5,
x_thresh=(30, 255),
y_thresh=(30, 255),
mag_thresh=(50, 255),
dir_thresh=(0.1, 1.4),
s_thresh=(140, 255)
)
# 4. keep region of interest
binary_img = region_of_interest(binary_img, vertices_roi)
# 5. warp image to bird's eye view perspective
warped_img = cv2.warpPerspective(binary_img, M, img_size, flags=cv2.INTER_LINEAR)
# 6. find lane lines
if current_left_line is None or current_right_line is None:
stats['fitSlidingWindow'] += 1
left_line, right_line = fitSlidingWindow(warped_img)
else:
stats['fitAroundPoly'] += 1
left_line, right_line = fitAroundPoly(warped_img, current_left_line, current_right_line)
# last chance before reset
if (not left_line.detected or not right_line.detected) and (failed_attempts > max_failed_attempts - 1) :
stats['fitSlidingWindow'] += 1
current_left_line = None
current_right_line = None
left_line, right_line = fitSlidingWindow(warped_img)
if sanityCheck(left_line, right_line) == True:
if current_left_line is None:
current_left_line = left_line
else:
current_left_line = current_left_line.append(left_line)
if current_right_line is None:
current_right_line = right_line
else:
current_right_line = current_right_line.append(right_line)
failed_attempts = 0
else:
stats['error'] += 1
failed_attempts += 1
# max failed attempts - then reset current_left_line, current_right_line
if failed_attempts > max_failed_attempts:
stats['reset'] += 1
current_left_line = None
current_right_line = None
if not left_line.detected:
stats['missingLeftLine'] += 1
if not right_line.detected:
stats['missingRightLine'] += 1
return current_left_line, current_right_line, undistort_img, binary_img, warped_img
# Do following line checks:
# - Checking is detected
# - Checking that they have similar curvature
# - Checking that they are separated by approximately the right distance horizontally
# - Checking that they are roughly parallel
def sanityCheck(left_line, right_line):
max_reldiff_radius_of_curvature = 1.5
max_delta_horizontally_distance = 0.3 # meter
max_mse_roughly_parallel = 30 # pixels
# Checking that they have similar curvature
reldiff_radius_of_curvature = reldiff(left_line.radius_of_curvature, right_line.radius_of_curvature)
# Checking that they are separated by approximately the right distance horizontally
delta_horizontally_distance = abs(left_line.line_base_pos + right_line.line_base_pos - 3.7)
# Checking that they are roughly parallel
ploty = np.linspace(500, 699, 200) # bottom of image
left_plotx = left_line.fitX(ploty)
right_plotx = right_line.fitX(ploty)
mse_roughly_parallel = ((right_plotx - left_plotx - 726)**2).mean()/200 # mean square error
'''
if reldiff_radius_of_curvature >= max_reldiff_radius_of_curvature:
print('FAIL reldiff_radius_of_curvature', reldiff_radius_of_curvature)
if delta_horizontally_distance >= max_delta_horizontally_distance:
print('FAIL delta_horizontally_distance', delta_horizontally_distance)
if mse_roughly_parallel >= max_mse_roughly_parallel:
print('FAIL mse_roughly_parallel', mse_roughly_parallel)
'''
#print(left_line.best_fit, right_line.best_fit)
return left_line.detected and right_line.detected and \
reldiff_radius_of_curvature < max_reldiff_radius_of_curvature and \
delta_horizontally_distance < max_delta_horizontally_distance and \
mse_roughly_parallel < max_mse_roughly_parallel
def reldiff(a, b):
return abs(a - b)/abs((a + b)/2)
def getErrorImage(image):
scaled_image = cv2.resize(image, (1280, 720))
undistort_image = cv2.undistort(scaled_image, mtx, dist, None, mtx)
putText(undistort_image, 'ERROR!', (50, 60), fontColor=(255,0,0))
return undistort_image
error = 0
def process_image(image):
global error
left_line, right_line, undistort_image, binary_image, warped_image = find_lane(image, 5)
#temp_img = warped_img*255
#return np.dstack((temp_img, temp_img, temp_img))
# handle missing line case
if left_line is None or right_line is None:
error += 1
error_image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
cv2.imwrite('output_images/error_{}.jpg'.format(error), error_image)
return getErrorImage(image)
lane_image, lane_warped = drawLane(image, warped_image, Minv, left_line, right_line)
annotate(lane_image, left_line, right_line)
return lane_image
In [15]:
if not os.path.exists('videos_output'):
os.makedirs('videos_output')
white_output = 'videos_output/project_video.mp4'
current_left_line = None
current_right_line = None
failed_attempts = 0
stats = {
'fitSlidingWindow': 0,
'fitAroundPoly': 0,
'error': 0,
'reset': 0,
'missingLeftLine': 0,
'missingRightLine': 0
}
clip1 = VideoFileClip("./project_video.mp4") #.subclip(13,15)
white_clip = clip1.fl_image(process_image)
%time white_clip.write_videofile(white_output, audio=False)
print(stats)
In [16]:
HTML("""
<video width="960" height="540" controls>
<source src="{0}">
</video>
""".format(white_output))
Out[16]: